//#define DEADZONE 

#define GAIN_XY 0.6 //Movimento x-y
#define GAIN_Z 0.8 //Rotazione su se stesso
#define ZMAXPULSE 800

void CalcolaVettore(int vx, int vy, double* res){
  if (vx > MAXVAL){
    vx = MAXVAL;
  }
  if (vx < -MAXVAL){
    vx = -MAXVAL;
  }
  if (vy > MAXVAL){
    vy = MAXVAL;
  }
  if (vy < -MAXVAL){
    vy = -MAXVAL;
  }
  
  double x = vx;
  double y = vy;
  x = x / MAXVAL;
  y = y / MAXVAL;
  /*
  double scale = sqrt(1+sqrt(x/y));
  if (x>y){
    scale = sqrt(1+sqrt(y/x));
  }
  */
  double scale = 1+square(x/y);
  if (abs(x)>abs(y)){
    scale = 1+square(y/x);
  }
  
  //double retval[2];
  double rad = atan2(y,x);
  double grad = rad/PI*180;
//  Serial.print("RAD:");
//  Serial.println(rad);
//  Serial.print("GRAD:");
//  Serial.println(grad);
  //double mod = sqrt((square(x)+square(y)) / scale);
  double mod = sqrt(x*x+y*y);
  if (mod > 1){
    mod = 1;
  }
  //mod = mod/scale;
  res[0] = rad;
  res[1] = mod;
  //return retval;
  //return 0;
}

void CalcolaServi(double* vettore, int* servo){
  //return 0;
  double rad = vettore[0];
  double mod = vettore[1];
  double scale = (ZEROPULSE-MINPULSE)*GAIN_XY/(PI/4);
  for (int i = 0; i< MAXSERVO; i++){
    double a = rad-((PI/8)+(PI/4)*i);
    double val = mod*sin(a);
    val = val*scale;
    //val = val*GAIN_XY;
    servo[i] = val+ZEROPULSE;
    //Serial.println(val);
  }
}

void CalcolaRotazione(int vz, int*servo){
  if (vz > MAXVAL){
    vz = MAXVAL;
  }
  if (vz < -MAXVAL){
    vz = -MAXVAL;
  }
  
  double z = vz;
  z = z / MAXVAL;
  
  
  for (int i = 0; i< MAXSERVO; i++){
    servo[i]+= ZMAXPULSE*z*GAIN_Z;
  }  
  
}
